#!/usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import division
import rospy
import time
import threading
import Adafruit_PCA9685
import RPi.GPIO as GPIO
from threading import Thread
from robot_pkg.msg import Person1


	
	
class WUS_motor(object):
	def __init__(self):
		self.Relays1_GPIO = 13 #继电器1GPIO 高电平使能
		self.Relays2_GPIO = 6 #继电器2GPIO 高电平使能
		self.Relays1flag = 0
		self.Relays2flag = 0
		self.current_throttle1 = 0;#左电机当前油门
		self.current_throttle2 = 0;#右电机当前油门
		self.current_throttle3 = 0;#吸污电机当前油门
		self.throttle1 = 0#左轮
		self.throttle2 = 0#右轮
		self.throttle3 = 0
		self.stop_thread = False
		#设置GPIO
		GPIO.setwarnings(False)
		GPIO.setmode(GPIO.BCM)
		GPIO.setup(self.Relays1_GPIO, GPIO.OUT)
		GPIO.output(self.Relays1_GPIO, 0)
		GPIO.setup(self.Relays2_GPIO, GPIO.OUT)
		GPIO.output(self.Relays2_GPIO, 0)
		
		self.pwm = Adafruit_PCA9685.PCA9685()
		self.pwm.set_pwm_freq(50)	#设置为50Hz频率
		
	
	#分成200份
	#-100~0 ：反转，0：停止，0~100：正转
	def set_servo_angle(self,channel,angle):
		angle  += 100
		date = int(4096*((angle*5)+1000)/20000+10)
		self.pwm.set_pwm(channel,0,date)
		time.sleep(0.001)
	
	#设置电机油门
	def set_throttle(self,channel,target_throttle,current_throttle):
		#print("目标油门:",channel,target_throttle)
		while not self.stop_thread:
			# 步长10%
			step_length = 1
			show_throttle=target_throttle

			if target_throttle != current_throttle :
				difference = target_throttle - current_throttle
				if abs(difference) < step_length:
					show_throttle = target_throttle
				else:
					if difference > 0:
						show_throttle = current_throttle + step_length
					else :
						show_throttle = current_throttle - step_length

			self.set_servo_angle(channel,show_throttle)
			# 记录当前油门
			if channel == 0:
				self.current_throttle1 = show_throttle
			if channel == 1:
				self.current_throttle2 = show_throttle
			if channel == 2:
				self.current_throttle3 = show_throttle
			current_throttle = show_throttle
			#print("当前油门:",channel,current_throttle)
			if target_throttle == current_throttle :
				return 0
	#控制继电器
	def ConrtolRelays(self,Relays_GPIO,data):
		self.stop_thread = False
		if data == 1:#打开
			GPIO.output(Relays_GPIO, 1)
			if Relays_GPIO == self.Relays1_GPIO:
				self.set_throttle(0,0,self.current_throttle1)#左轮
				self.set_throttle(1,0,self.current_throttle2)#右轮
			if Relays_GPIO == self.Relays2_GPIO:
				self.set_throttle(2,0,self.current_throttle3)
				time.sleep(3)
			time.sleep(1)
		if data == 2:#关闭
			if Relays_GPIO == self.Relays1_GPIO:
				self.set_throttle(0,0,self.current_throttle1)#左轮
				self.set_throttle(1,0,self.current_throttle2)#右轮
			if Relays_GPIO == self.Relays2_GPIO:
				self.set_throttle(2,0,self.current_throttle3)
			time.sleep(1)
			GPIO.output(Relays_GPIO, 0)
	
	#错误处理	
	def Dispose_error(self):
		self.stop_thread = False
		#关闭电机
		self.set_throttle(0,0,self.current_throttle1)#左轮
		self.set_throttle(1,0,self.current_throttle2)#右轮
		self.set_throttle(2,0,self.current_throttle3)
		#关闭继电器
		print("关闭继电器")
		GPIO.output(self.Relays1_GPIO, 0)
		GPIO.output(self.Relays2_GPIO, 0)
	#定时器
	def tesk_timer(self,rocker):
		if(rocker != 0):
			#关闭电机
			self.set_throttle(0,0,self.current_throttle1)#左轮
			self.set_throttle(1,0,self.current_throttle2)#右轮
			self.set_throttle(2,0,self.current_throttle3)
	
	def get_data(self,msg):	
		self.stop_thread = True
		#控制继电器
		if msg.Relays1 != self.Relays1flag:
			self.Relays1flag = msg.Relays1
			self.ConrtolRelays(self.Relays1_GPIO,msg.Relays1)
		if msg.Relays2 != self.Relays2flag:
			self.Relays2flag = msg.Relays2
			self.ConrtolRelays(self.Relays2_GPIO,msg.Relays2)	
		#控制左右电机
		self.throttle1 = 0#左轮
		self.throttle2 = 0#右轮
		if(msg.Rocker == 0):#停止
			self.throttle1 = 0
			self.throttle2 = 0
		if(msg.Rocker == 1):#前进
			self.throttle1 = msg.Throttle
			self.throttle2 = msg.Throttle
		if(msg.Rocker == 2):#后退
			self.throttle1 = -msg.Throttle
			self.throttle2 = -msg.Throttle
		if(msg.Rocker == 3):#右转
			self.throttle1 =  msg.Throttle
			self.throttle2 = -msg.Throttle
		if(msg.Rocker == 4):#左转
			self.throttle1 = -msg.Throttle
			self.throttle2 =  msg.Throttle
		#控制吸污电机 档位1~5
		self.throttle3 = 0
		if msg.Gears < 6: 
			self.throttle3 = msg.Gears * 20 
		#出现错误
		if msg.error == 1:
			print("左右电机关闭")
			self.Dispose_error()
		

	def main(self):
		rospy.init_node('WUS_motor', anonymous=True)
		#链接服务器，获取摇杆信息
		rospy.Subscriber("/client_publisher", Person1, self.get_data,queue_size=100)

		# #设置循环的频率
		rate = rospy.Rate(10) 
		while not rospy.is_shutdown():
			self.stop_thread = False
			thread1 = Thread(target=self.set_throttle,args=(0,self.throttle1,self.current_throttle1,))
			thread2 = Thread(target=self.set_throttle,args=(1,self.throttle2,self.current_throttle2,))
			thread3 = Thread(target=self.set_throttle,args=(2,self.throttle3,self.current_throttle3,))
			#线程开始
			thread1.start()
			thread2.start()
			thread3.start()	
			rate.sleep()
	



if __name__ == '__main__':
	try :    
		p = WUS_motor()
		p.main()
	except rospy.ROSInterruptException:
		pass
    
    
    
    
    
    
